
#include "LSM303DLMag.h"
#include "..\System\F2806x_Device.h"
#include <math.h>

bool LSM303DLMag::Init()
{
	// init I2C Driver
	m_SAR = 0x001E; // LSM303DLH - Magneto I2C address
	InitI2C();
		
	// set ctrl regA
	Uint16 ctrlA;
	ctrlA = 0x18; // Data output: 75Hz (DO2 = 1, DO1 = 1)
	WriteReg8(0x00, ctrlA);
	
	// set ctrl regB
	Uint16 ctrlB;
	ctrlB = 0x20; // range = +1.3gauss, GN0 = 1
	WriteReg8(0x01, ctrlB);
	
	// set ctrl regMR
	Uint16 ctrlMR;
	ctrlMR = 0x00; // power on device
	WriteReg8(0x02, ctrlMR);
	
	return true;
}

void LSM303DLMag::Process()
{
	int16 axes[3];

	ReadReg16(0x03, (Uint16*)&axes[0] ); // WTF?!?! (Y/Z swapped?, address write delayed?)
	ReadReg16(0x07, (Uint16*)&axes[1] );
	ReadReg16(0x05, (Uint16*)&axes[2] );
	m_AxisX = axes[0] / 1055.0f; // 1055 LSB/Gauss (+-1.3gauss range)
	m_AxisY = axes[1] / 1055.0f; // 1055 LSB/Gauss (+-1.3gauss range)
	m_AxisZ = axes[2] / 950.0f;   // 950 (Zaxis) LSB/Gauss (+-1.3gauss range)
}
